/*  Mash-code. Mash Arts & Métiers Angers embedded software
 *  Copyright (C) 2014  Mash Arts & Métiers Angers
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/* 
 *  Main Arduino v1
 *
 * 
 *  Fonctionalitées : -Read/Write I2C compatible avec le contoleur de vitesse
 *                    -Retrour via port series
 * 
 */

/******************************************************************************/
/* Files to Include                                                           */
/******************************************************************************/
 
#include <Wire.h>

/******************************************************************************/
/* Global Variable Declaration                                                */
/******************************************************************************/

//Definition des pins
#define LED_ROUGE 1
#define LED_ORANGE 2
#define LED_VERTE 3

#define BTN_MOT 4
#define BTN_FREIN_AV 5
#define BTN_FREIN_AR 6

#define INT_CAP_ROUE 5

#define PTR_MOT 12

//Define I2C
#define DEVICE_ADRESS 0x50
#define DEVICE_STATE 1
#define DEVICE_PWM 2

//define Timer1
#define T1V 49910

#define D_PNEU 1,495
#define TOP_P_TOUR 8

bool debug = false;
bool lastBrk = false;
bool currentMot = false;
bool currentBrk = false;

volatile unsigned int topRoue = 0;
volatile float distance = 0;
volatile float vitesse = 0;
volatile float vitesseMoy = 0;

/******************************************************************************/
/* Interruption capteur                                                       */
/******************************************************************************/

void capteurRoue()
{
	topRoue++;
}

/******************************************************************************/
/* It's a Trap !                                                              */
/******************************************************************************/

void trap()
{
	detachInterrupt(INT_CAP_ROUE);
	
	TIMSK1 = 0;
	
	writeI2C(DEVICE_ADRESS, DEVICE_PWM, 0);
	
	for(;;)
	{
		//allumage des leds
		digitalWrite(LED_ROUGE, HIGH);
		digitalWrite(LED_ROUGE, HIGH);
		digitalWrite(LED_VERTE, HIGH);
		
		delay(500);
		
		//extinction des leds
		digitalWrite(LED_ROUGE, LOW);
		digitalWrite(LED_ROUGE, LOW);
		digitalWrite(LED_VERTE, LOW);
		
		delay(500);
	}
}

/******************************************************************************/
/* I2C Declaration                                                            */
/******************************************************************************/

int readI2C(int adrDevice, int adrReg) {
  Wire.beginTransmission(adrDevice); 
  Wire.write(adrReg);
  Wire.endTransmission();
  Wire.requestFrom(adrDevice, 1);
  while(Wire.available() == 0);
  int c = Wire.read();   
  return c;
}

int writeI2C(int adrDevice, int adrReg, uint8_t data) {
  Wire.beginTransmission(adrDevice);
  Wire.write(adrReg);
  Wire.write(data);
  Wire.endTransmission();
}

/******************************************************************************/
/* Interupt Service Routine                                                   */
/******************************************************************************/

ISR(TIMER1_OVF_vect)
{
	TCNT1 = T1V;
	
	vitesse = 3,6 * D_PNEU * topRoue / TOP_P_TOUR;
	ditance += D_PNEU * topRoue / TOP_P_TOUR;
	vitesseMoy = 3600 * distance / millis();

	digitalWrite(LED_ROUGE, LOW);
	digitalWrite(LED_ROUGE, LOW);
	digitalWrite(LED_VERTE, LOW);
	
	if(vitesseMoy < 25.0)
		digitalWrite(LED_ROUGE, HIGH);
	else if(vitesseMoy >= 25.0 && vitesseMoy < 28.8)
		digitalWrite(LED_VERTE, HIGH);
	else
		digitalWrite(LED_ROUGE, HIGH);
}

/******************************************************************************/
/* Initialisation                                                             */
/******************************************************************************/

void setup()
{
	Wire.begin();
	Serial.begin(115200);
	debug = Serial.available();
	
	//configuration des pins
	pinMode(LED_ROUGE, OUTPUT);
	pinMode(LED_ORANGE, OUTPUT);
	pinMode(LED_VERTE, OUTPUT);
	pinMode(BTN_MOT, INPUT);
	pinMode(BTN_FREIN_AV, INTPUT);
	pinMode(BTN_FREIN_AR,INTPUT);
	
	//allumage des leds
	digitalWrite(LED_ROUGE, HIGH);
	digitalWrite(LED_ROUGE, HIGH);
	digitalWrite(LED_VERTE, HIGH);
	
	//Init Speed controleur
	writeI2C(DEVICE_ADRESS, DEVICE_PWM, 0);
	
	//extinction des leds
	digitalWrite(LED_ROUGE, LOW);
	digitalWrite(LED_ROUGE, LOW);
	digitalWrite(LED_VERTE, LOW);
	
	// Initialisation des interruptions sur le Timer 1
	noInterrupts() ;
		TCCR1A = 0 ;
		TCCR1B = 0 ;
		TCNT1 = T1V ;
		TCCR1B |= 101 ;
		TIMSK1 |= 1 ;
	interrupts() ;
	
	attachInterrupt(INT_CAP_ROUE, capteurRoue, RISING);
}

/******************************************************************************/
/* Main Loop                                                                  */
/******************************************************************************/

void loop()
{
	currentMot = digitalRead(BTN_MOT);
	currentBrk = digitalRead(BTN_FREIN_AR) || digitalRead(BTN_FREIN_AV);
	
	if(lastBrk && currentMot) currentBrk=true;
	
	currentMot = currentMot && !currentBrk;
	lastBrk = currentBrk;
	
	if(currentMot)
	{
		writeI2C(DEVICE_ADRESS, DEVICE_PWM, analogRead(PTR_MOT)>>2);
	}
	else
	{
		writeI2C(DEVICE_ADRESS, DEVICE_PWM, 0);
	}
	
	if(readI2C(DEVICE_ADRESS, DEVICE_STATE)<=0) trap();
	
	delay(50);
}
